#include <ctime>

#include "Fitting.h"

namespace rock {

auto Fitting::least_sqaure() -> plane_type {

    //  平面方程的一般表达式为：Ax + By + Cz + D = 0
    //  移项得到：z = (-A / C) * x + (-B / C) * y + (-D / c)
    //  令 :
    // 	   a0 = -A / C,
    //     a1 = -B / C,
    //     a2 = -D / c
    //  平面方程表达式变为：z = a0 * x + a1 * y + a2
    //  这里用最小二乘法求出 a0, a1, a2,
    //  然后再得出平面的法向量和归一化法向量

    if (points_.size() == 0)
        return plane_type{};

    Eigen::Matrix3d H = Eigen::Matrix3d::Zero();
    Eigen::Vector3d b = Eigen::Vector3d::Zero();

    double H00 = 0.0, H01 = 0.0, H02 = 0.0, H11 = 0.0, H12 = 0.0;
    double H22 = points_.size();
    double x, y, z;

    // 得到系数H 和 b
    for (auto it = points_.begin(); it != points_.end(); ++it) {
        x = (*it)(0, 0);
        y = (*it)(1, 0);
        z = (*it)(2, 0);
        H00 += x * x;
        H01 += x * y;
        H02 += x;
        H11 += y * y;
        H12 += y;

        b(0, 0) += x * z;
        b(1, 0) += y * z;
        b(2, 0) += z;
    }

    H << H00, H01, H02, 
         H01, H11, H12, 
         H02, H12, H22;

    // H 对称正定，使用ldlt加速求解
    Eigen::Vector3d coeff = H.ldlt().solve(b);

    // 输出 plane_type 结果
    plane_type result = Eigen::Vector4d::Zero();
    result << -coeff(0, 0), -coeff(1, 0), 1, -coeff(2, 0);

    return result;
    }

auto Fitting::ransac() -> plane_type {
    if (points_.size() == 0)
        return plane_type{};

    int maxinliers = 0;
    srand((unsigned)time(0));

    plane_type best_result = Eigen::Vector4d::Zero();

    // 迭代500次
    for (int i = 0; i < 1000; i++) {

        // 步骤1：从点云数据中随机选择3个点
        std::vector<int> vec_index;
        while (1) {
            int r = rand() % (points_.size());
            vec_index.push_back(r);
            if (vec_index.size() == 3)
                break;
        }

        // 步骤2：通过这3个点的数据求解平面模型参数
        Eigen::Matrix3d H = Eigen::Matrix3d::Zero();
        Eigen::Vector3d b = Eigen::Vector3d::Zero();
        // z = ax + by + c
        H << points_[vec_index[0]](0, 0),   points_[vec_index[0]](1, 0),    1,
             points_[vec_index[1]](0, 0),   points_[vec_index[1]](1, 0),    1,
             points_[vec_index[2]](0, 0),   points_[vec_index[2]](1, 0),    1;

        b << points_[vec_index[0]](2, 0),   points_[vec_index[1]](2, 0),    points_[vec_index[2]](2, 0);

        Eigen::Vector3d coeff = H.inverse() * b;
        // std::cout << coeff.transpose() << std::endl;

        // 步骤3：统计内点个数
        int inliers = 0;
        int sigma   = 2.0; // 阈值，可以自己调整

        plane_type coeff4d = Eigen::Vector4d::Zero();
        coeff4d << -coeff(0, 0), -coeff(1, 0), 1, -coeff(2, 0);

        plane_type xyz1 = Eigen::Vector4d::Zero();

        for (int k = 0; k < points_.size(); k++) {
            xyz1 << points_[k], 1;
            double distance = fabs(coeff4d.dot(xyz1));

            if (distance < sigma)
                inliers++;
        }
        // std::cout << "inliers: " << inliers << std::endl;

        // 步骤4：选出内点个数最大的参数
        if (inliers > maxinliers) {
            maxinliers = inliers;
            best_result = coeff4d;
        }
    }

    std::cout << "max inliers: " << maxinliers << std::endl;

    return best_result;
}
} // namespace rock
